# 导入所需的模块
import sensor, image, time, math
from pyb import UART

# 红色激光灯在纸上颜色的阈值，用于颜色识别，可以根据实际情况进行调节
Red_threshold =(0, 100, 25, 55, -128, 127)

# 绿色激光灯在纸上颜色的阈值，用于颜色识别，可以根据实际情况进行调节
Green_threshold = (0, 100, -128, -32, -128, 127)

# 初始化UART通信，用于与OpenMV Cam进行数据传输
uart = UART(3, 9600)  # UART 3是OpenMV Cam的UART编号，9600是波特率

# 重置传感器，设置图像格式和分辨率
sensor.reset()
sensor.set_pixformat(sensor.RGB565)  # 设置图像格式为RGB565
sensor.set_framesize(sensor.QQVGA)  # 设置分辨率为QQVGA，减小分辨率可以减少内存使用

# 选择VGA分辨率中的中心像素区域，减少处理数据量
sensor.set_windowing((320, 300))

# 关闭自动增益，以避免图像亮度自动调整
sensor.set_auto_gain(False)

# 让新设置生效，等待20帧
sensor.skip_frames(20)

# 关闭自动白平衡，以保持颜色的一致性
sensor.set_auto_whitebal(False)

# 创建一个时钟对象，用于控制帧率
clock = time.clock()

# 用于存储从UART接收到的数据
a = '0'

# 设置实心圆的大小（半径）
red_radius = 3    # 红色实心圆半径
green_radius = 4  # 绿色实心圆半径

# 主循环
while(True):
    clock.tick()  # 更新时钟

    # 如果UART有数据
    if uart.any():
        a = uart.readline()  # 读取数据

    img = sensor.snapshot()  # 拍摄一帧图像

    # 定义一个函数，用于查找图像中的色块并返回其中心坐标
    def color_blob(threshold, color):
        blobs = img.find_blobs([threshold])  # 查找图像中的色块
        if blobs:  # 如果找到色块
            b = blobs[0]  # 获取色块信息
            cx = b[5]  # 获取色块中心的x坐标
            cy = b[6]  # 获取色块中心的y坐标
            # 根据颜色参数绘制不同颜色的实心圆
            if color == "red":
                img.draw_circle(cx, cy, red_radius, color=(255, 0, 0), fill=True)  # 绘制红色实心圆
            elif color == "green":
                img.draw_circle(cx, cy, green_radius, color=(0, 255, 0), fill=True)  # 绘制绿色实心圆
            return cx, cy  # 返回色块中心坐标
        return 160, 120  # 如果没有找到色块，返回默认值

    # 查找红色激光并用红色实心圆标记
    red_blob = color_blob(Red_threshold, "red")
    # 查找绿色激光并用绿色实心圆标记
    green_blob = color_blob(Green_threshold, "green")

    # 计算红绿激光点中心距离
    distance = math.sqrt((red_blob[0] - green_blob[0])**2 + (red_blob[1] - green_blob[1])**2)

    # 发送红色激光数据
    if red_blob != (160, 120) and distance >= 1:
        x = red_blob[0] // 255
        y = red_blob[1] // 255
        data = bytearray([0xFD, x, red_blob[0], y, red_blob[1], 0xFA])  # 构造数据包
        uart.write(data)  # 通过UART发送数据
        print(data)  # 打印数据

    # 发送绿色激光数据
    if green_blob != (160, 120) and distance >= 1:
        x = green_blob[0] // 255
        y = green_blob[1] // 255
        data = bytearray([0xAB, x, green_blob[0], y, green_blob[1], 0xCD])  # 构造数据包
        uart.write(data)  # 通过UART发送数据
        print(data)  # 打印数据


    # 如果接收到的数据大于'0'，则进入下一个循环
    while (a > '0'):
        if uart.any():  # 如果UART有数据
            a = uart.readline()  # 读取数据
